#ifndef __HAND_DETECTOR__
#define __HAND_DETECTOR__
#include "Renderer.h"
#include "OpenCLGL_Util.h"
#include "Conio.h"
#include <thread>
#include "CostFunction.h"
#include "ParticleSwarm.h"

enum DetectMode
{
	DETECT_MODE_FIRST_FRAME,
	DETECT_MODE_TRACKING
};
using namespace std;

class HandDetector3D
{
public:
	HandDetector3D(GLFWwindow * pWindow);
	~HandDetector3D(void);
	

	void setObservation(cv::Mat depth, cv::Mat color, cv::Vec3d handPos, BOOL needPredictWrist);
	void trackingFrame(int maxIteration);
	void trackingFirstFrame();
	void trackingNextFrame();

	void drawResultToMat(cv::Mat rawImg, cv::Rect rect);
	GLRenderer renderer;
	double currentOptimize;

	/*NEED UPDATE AFTER TRACKING !!!*/
	vector<double>	lastestPos;
	Hypothesis		previewHypo;
	cv::Vec3d 		centerPosGT;
	cv::Vec3d		wristPosGT; //Position from ground truth
	cv::Mat previewHandImage;
	void calculateGroundTruth();// new WRIST_GT = center + real_WRIST 
#ifdef ALGORITHMS_SIMULATE
	void runTest();
#endif

protected:
	DetectMode detectMode;
	Hypothesis currentPose;

private:
	
	CostFunction costFunction;
	ParticleSwarm swarm;
	OpenCLManager oclManager;

	Hypothesis previousPose;
	
	cv::Mat depthConvertTable;
	float kinect_focal_length;

	void predictHandWrist();
};

#endif
